#include "svpwm.h"
#include <stdio.h>

#define sq3 1.73205080757f
void SVPWM(float alpha, float beta, float *d)
{
	// alpha *= 0.866;
	// beta *= 0.866;
	
	//  2. 计算中间变量
	float X = beta * 2.0f / sq3;
	float Y = alpha - beta / sq3;
	float Z = alpha + beta / sq3;

	printf("X:%f Y:%f Z:%f\n", X, Y, Z);

	// 3. 扇区判断与统一计算
	int sector = (X >= 0) ? ((Y < 0) ? ((Z >= 0) ? 2 : 3) : 1)
						  : ((Y >= 0) ? ((Z >= 0) ? 6 : 5) : 4);

	float T1, T2;
	// clang-format off
    switch (sector) {
        case 1: T1 =  Y;  T2 =  X;  break;
        case 2: T1 = -Y;  T2 =  Z;  break;
        case 3: T1 =  X;  T2 = -Z;  break;
        case 4: T1 = -X;  T2 = -Y;  break;
        case 5: T1 = -Z;  T2 =  Y;  break;
        case 6: T1 =  Z;  T2 = -X;  break;
    }
	// clang-format on
	printf("T1:%f T2:%f\n", T1, T2);
	// 4. 计算占空比（七段式对称调制）
	float T0 = 1.0f - T1 - T2;
	float d120, d20, d0;
	d120 = T1 + T2 + T0 / 2; // Ta
	d20 = T2 + T0 / 2;		 // Tb
	d0 = T0 / 2;			 // Tc
	// clang-format off
    switch (sector) {
        case 1: d[0] = d120;  d[1]=d20;	  d[2]=d0;   break;
        case 2: d[0] = d20;	  d[1]=d120;  d[2]=d0;   break;
        case 3: d[0] = d0;	  d[1]=d120;  d[2]=d20;  break;
        case 4: d[0] = d0;	  d[1]=d20;	  d[2]=d120; break;
        case 5: d[0] = d20;	  d[1]=d0;	  d[2]=d120; break;
        case 6: d[0] = d120;  d[1]=d0;	  d[2]=d20;  break;
    }
	// clang-format on
}
